/**
  ******************************************************************************
  * @file	uArmDebug.cpp
  * @author	David.Long	
  * @email	xiaokun.long@ufactory.cc
  * @date	2016-12-02
  ******************************************************************************
  */

#include "uArmDebug.h" 
#include <stdarg.h>


#define ARDBUFFER 50

int msprintf(char *result, char *str, ...)
{
	int i, count=0, j=0, flag=0;
	char temp[ARDBUFFER+1];
	char num[ARDBUFFER+1];
	
	for(i=0; str[i]!='\0';i++)  
	{
		if(str[i]=='%')  
			count++;
	}

    result[0] = '\0';

	if (count == 0)
	{
		for (i = 0; str[i] != '\0'; i++)
		{
			result[i] = str[i];
		}
		result[i] = '\0';
	}
	else
	{

		va_list argv;
		va_start(argv, count);
		for(i=0,j=0; str[i]!='\0';i++)
		{
			if(str[i]=='%')
			{
				temp[j] = '\0';
				strcat(result, temp);
				j=0;
				temp[0] = '\0';

				switch(str[++i])
				{
					case 'd': 
					{

						itoa(va_arg(argv, int), num, 10);
						strcat(result, num);
						break;
					}

					case 'l': 
						ltoa(va_arg(argv, long), num, 10);
						strcat(result, num);
						break;

					case 'f': 
					{
						char d_str[10];
						dtostrf(va_arg(argv, double), 4, 2, d_str);
						strcat(result, d_str);
						break;
					}

					case 'c': 
						num[0] = (char)va_arg(argv, int);
						num[1] = '\0';
						strcat(result, num);
						break;
						
					case 's': 
						strcat(result, va_arg(argv, char *));
						break;

					default:  
						break;
				}
			}
			else 
			{
				temp[j] = str[i];
				j = (j+1)%ARDBUFFER;
				if(j==0) 
				{
					temp[ARDBUFFER] = '\0';
					strcat(result, temp);
					temp[0]='\0';
				}
			}
		}
	}
	temp[j] = '\0';
	strcat(result, temp);

	return count + 1;
}



void mprint(char *fmt, ...) 
{
	char buf[PRINT_BUF] = {0};

	int i, count=0, j=0, flag=0;
	char temp[ARDBUFFER+1];
	char num[ARDBUFFER+1];

	for(i=0; fmt[i]!='\0';i++) 
	{
		if(fmt[i]=='%')  
			count++;
	}

	if (count == 0)
	{
		for (i = 0; fmt[i] != '\0'; i++)
		{
			buf[i] = fmt[i];
		}
		buf[i] = '\0';
	}
	else
	{
		buf[0] = '\0';
		va_list argv;
		va_start(argv, count);
		for(i=0,j=0; fmt[i]!='\0';i++)
		{ 
			if(fmt[i]=='%')
			{
				temp[j] = '\0';
				strcat(buf, temp);
				j=0;
				temp[0] = '\0';

				switch(fmt[++i])
				{
					case 'd': 
					{
						itoa(va_arg(argv, int), num, 10);
						strcat(buf, num);
						break;
					}

					case 'l': 
						ltoa(va_arg(argv, long), num, 10);
						strcat(buf, num);
						break;

					case 'f': 
					{
						char d_str[10];
						dtostrf(va_arg(argv, double), 4, 2, d_str);
						strcat(buf, d_str);
						break;
					}

					case 'c': 
						num[0] = (char)va_arg(argv, int);
						num[1] = '\0';
						strcat(buf, num);
						break;

					case 's': 
						strcat(buf, va_arg(argv, char *));
						break;
					default:  
						break;
				};
			}
			else 
			{
				temp[j] = fmt[i];
				j = (j+1)%ARDBUFFER;
				if(j==0) 
				{
					temp[ARDBUFFER] = '\0';
					strcat(buf, temp);
					temp[0]='\0';
				}
			}
		}
	}


	temp[j] = '\0';
	strcat(buf, temp);

	PrintSerial.print(buf);

}


